perm filename LS1V3P.SAI[1,BGB]1 blob sn#001257 filedate 1972-10-22 generic text, type T, neo UTF8
00100	ENTRY LS1V3P,GAP;
00200	BEGIN "LS1V3P"
00300		REQUIRE "ABBREV[SYS,BGB]" SOURCE_FILE;
00400		REQUIRE "TRIGER[SYS,BGB]" SOURCE_FILE;
00500	α IMPLICIT RESULTS;
00600		REAL D,WWW;
00700	α COMPONENTS OF VIEWPOINT AND LANDMARK VECTORS;
00800		REAL X,Y,Z,XA,YA,ZA,XB,YB,ZB,XC,YC,ZC;
00900	α COSINES OF RAYS TO THE 3 LANDMARK POINTS FROM THE VIEWPOINT;
01000		REAL COSα,COSβ,COSg;
01100	α COMPONENTS OF LANDMARK TRIANGLE SIDE VECTORS;
01200		REAL DXA,DYA,DZA, DXB,DYB,DZB, DXC,DYC,DZC;
01300	α THE LENGTH OF THE SIDES OF THE LANDMARK TRIANGLE;
01400		REAL A,B,C;
01500	α IMPLICIT GAP VALUES;
01600		REAL XX,ZZ;
01700	α INIT IMPLICIT GAP ARGUMENTS;
01800		REAL CTHETA,STHETA,COSPHI,SINPHI,TANPSI;
01900	α COMPUTE THE ANGLES PHI AND PSI;
02000	PROCEDURE PHIPSI (REAL Cα,Cβ,Cg);
02100	BEGIN	"PHIPSI"
02200		REAL RAD,Sα,COSPSI,SINPSI,THETA,PHI,PSI;
02300		RAD	←	SQRT(Cg↑2 - 2*Cα*Cβ*Cg + Cβ↑2);
02400		Sα	←	SQRT(1-Cα↑2);
02500		COSPHI	←	Sα*Cβ/RAD;
02600		SINPHI	←	SQRT(1-COSPHI↑2);
02700		COSPSI	←	RAD/Sα;
02800		SINPSI	←	SQRT(1-COSPSI↑2);
02900		TANPSI	←	SINPSI/COSPSI;
02950		PHI	←	ACOS(COSPHI);
03000		THETA	←	ACOS(Cα) - ACOS(COSPHI);
03025		PSI	←	ACOS(COSPSI);
03200		STHETA	←	SIN(THETA);
03300		CTHETA	←	COS(THETA);
03400	END	"PHIPSI";
     

00100	α THE GAP FUNCTION SUBROUTINE  -  THE CRUX OF LS1V3P IS TO FIND GAP ROOTS;
00150		REAL WHI,WLO;
00200		REAL     R,S,H;
00300	INTERNAL REAL PROCEDURE GAP (REAL W);
00400	BEGIN	"GAP"
00500		REAL RSW,RCW,DX,DY,K;
00600		REAL XX1,DY1,XX2,DY2;
00700	α L VECTOR;
00800		RSW	←	R*SIN(W);
00900		RCW	←	R*COS(W);
01000	α (B-L) VECTOR WHEN W<0, (C-L) VECTOR WHEN W≥0;
01100		DX	←	-S-RCW;
01200	α PHI ROTATION CCW OF (B-L) VECTOR;
01300	BEGIN
01400		DY	←	+A/2-RSW;
01500		XX1	←	COSPHI*DX - SINPHI*DY;
01600		DY1	←	COSPHI*DY + SINPHI*DX;
01700	END;
01800	α THETA ROTATION CW OF (C-L) VECTOR;
01900	BEGIN
02000		DY	←	-A/2-RSW;
02100		XX2	←	CTHETA*DX + STHETA*DY;
02200		DY2	←	CTHETA*DY - STHETA*DX;
02300	END;
02500		IF W<0 THEN BEGIN DY←DY1;XX←XX1;END
02600		       ELSE BEGIN DY←DY2;XX←XX2;END;
02700	α PSI ROTATION FOR Z COMPONENT;
02800		Z	←	SQRT(XX↑2+DY↑2)*TANPSI;
02900	α SOLVE FOR XX & ZZ;
03000		K	←	(D-RSW)/DY;
03100		XX	←	RCW + K*XX;
03200		ZZ	←	K*Z;
03300		RETURN(SQRT((XX+S)↑2 + ZZ↑2)- H);
03400	END	"GAP";
     

00100	α GET THE ANSWER BACK INTO THE ORIGINAL FRAME OF REFERANCE;
00200	PROCEDURE OFRAME;
00300	BEGIN	"OFRAME"
00400		DEFINE	determinant (a11,a12,a13,a21,a22,a23,a31,a32,a33)=
00500		"(a11*(a22*a33-a23*a32)-a12*(a21*a33-a23*a31)+a13*(a21*a32-a22*a31))";
00600		REAL RCW,RSW,SXX,A2,RCWX,RSWD,PA2D,MA2D,II,JJ,KK,KA,KB,KC,K;
00700		REAL LA,LB,LC,G;
00800		IF ZZ<0 THEN OUTSTR("ZZ NEGATIVE."&13&10);
00900		RCW 	←	 R*COS(WWW);
01000		RSW 	←	 R*SIN(WWW);
01100	α COMPUTE THE LENGTHS OF THE LEGS OF THE TRIPOD;
01200		LA  	←	 SQRT((RCW-XX)↑2 + (RSW-D)↑2 + ZZ↑2);
01300		LB  	←	 SQRT((RCW+S)↑2 + (RSW-A/2)↑2);
01400		LC  	←	 SQRT((RCW+S)↑2 + (RSW+A/2)↑2);
01500	α COMPUTE SOME OF THE COMPONENTS OF THE LEG VECTORS,
01600	  WITH RESPECT TO THE CHORD'S MIDPOINT FRAME OF REFERENCE;
01700		A2	←	A/2;
01800		SXX	←	-S-XX;
01900		RCWX	←	RCW-XX;
02000		RSWD	←	RSW-D;
02100		PA2D	←	A2-D;
02200		MA2D	←	-A2-D;
02300	α COMPONENTS OF A VECTOR NORMAL TO THE PLANE OF THE LANDMARK TRIANGLE;
02400		II	←	DZB*DYC - DYB*DZC;
02500		JJ	←	DXB*DZC - DZB*DXC;
02600		KK	←	DYB*DXC - DXB*DYC;
02700	α COMPUTE YE OLDE TRIPLE PRODUCT OF (C TO A) CROSS (C TO B) DOT (C TO L);
02800		KA	←	-DXC*XA - DYC*YA - DZC*ZA + SXX*RCWX + PA2D*RSWD +ZZ↑2;
02900		KB	←	 DXB*XA + DYB*YA + DZB*ZA + SXX*RCWX + MA2D*RSWD +ZZ↑2;
03000		KC	←	  II*XA +  JJ*YA +  KK*ZA + A*ZZ*(RCW + S);
03100	α APPLY CRAMER'S RULE;
03200		K	←	DETERMINANT(-DXC, -DYC, -DZC,
03300					     DXB,  DYB,  DZB,
03400				   	      II,   JJ,   KK);
03500		X	←	DETERMINANT(  KA, -DYC, -DZC,
03600					      KB,  DYB,  DZB,
03700				   	      KC,   JJ,   KK) / K;
03800		Y	←	DETERMINANT(-DXC,   KA, -DZC,
03900					     DXB,   KB,  DZB,
04000				   	      II,   KC,   KK) / K;
04100		Z	←	DETERMINANT(-DXC, -DYC,   KA,
04200					     DXB,  DYB,   KB,
04300				   	      II,   JJ,   KC) / K;
04350		IF ABS(K)≤0.01 THEN OUTSTR("KRAMER K WARNING	"&CVG(K)&13&10);
04400	END	"OFRAME";
     

00100	α LOCUS SOLUTION:  1 VIEW OF 3 POINTS CASE;
00200	α LS1V3P RETURNS -1 FOR NO SOLUTIONS GAP LOW, 0 FOR NO SOLUTIONS GAP HIGH,
00300	   OTHERWISE N FOR THAT MANY SOLUTIONS STASHED IN ARRAY V[1:N,1:3];
00400	INTERNAL INTEGER PROCEDURE LS1V3P (REAL ARRAY V,ARGS);
00500	BEGIN	"LS1V3P"
00600		INTEGER NROOTS;
00700	α STASH ARGUMENT ARRAYS INTO WORKING VARIABLES;
00800		ARRBLT(XA,ARGS[1,1],12);
01200	α COMPUTE THE SIDES OF THE LANDMARK TRIANGLE;
01300		DXA ← XB - XC;		DYA ← YB - YC;		DZA ← ZB - ZC;
01400		DXB ← XC - XA;		DYB ← YC - YA;		DZB ← ZC - ZA;
01500		DXC ← XA - XB;		DYC ← YA - YB;		DZC ← ZA - ZB;
01600	α COMPUTE LENGTHS OF THE SIDES OF THE LANDMARK TRIANGLE;
01700		A ← SQRT(DXA↑2 + DYA↑2 + DZA↑2);
01800		B ← SQRT(DXB↑2 + DYB↑2 + DZB↑2);
01900		C ← SQRT(DXC↑2 + DYC↑2 + DZC↑2);
01902	α CHECK FOR COLINEAR CASE;
01904	BEGIN
01906		REAL A,B,C,D;
01908		A	←	ACOS(COSα);
01910		B	←	ACOS(COSβ);
01912		C	←	ACOS(COSG);
01914		IF B>A THEN A↔B;
01916		IF C>A THEN A↔C;
01917		D	←	ABS(B+C-A);
01918		IF D≤0.005 THEN OUTSTR("COLINEAR WARNING	"&CVG(D)&13&10);
01920	END;
02000	BEGIN	"2ND BLK"
02100		REAL SINα,COSC,ALPHA,W,GLO,GHI,G;
02200	α R IS THE RADIUS OF L'S CIRCLE;
02300	α S IS THE DISTANCE TO THE AB CHORD FROM THE CENTER OF L'S CIRCLE;
02400		SINα	←	SQRT(1-COSα↑2);
02500		R	←	A/(2*SINα);
02600		S	←	R*COSα;
02700	α H IS THE ALTITUDE OF THE LANDMARK TRIANGLE FROM C TO SIDE AB;
02800	α H IS ALSO THE RADIUS OF THE SPINDLE;
02900		COSC	←	(A↑2 + B↑2 - C↑2)/(2*A*B);
03000		H	←	B*SQRT(1 - COSC↑2);
03100	α D IS THE DIRECTED DISTANCE FROM THE FOOT OF THE ALTITUDE,
03200	  TO THE MIDPOINT OF THE CHORD AB;
03300		D	←	B*COSC - A/2;
     

00100	α FIND ZERO CROSSINGS;
00200	BEGIN	"3RD BLK"
00300		REAL PROCEDURE ROOT (REAL WLO,WHI);
00400	BEGIN	"ROOT"
00500		REAL GLO,GHI,W,G;
00600		GLO	←	GAP(WLO);
00700		GHI	←	GAP(WHI);
00800		DO
00900	BEGIN	"HALVE INTERVAL" 
01000		W	←	(WLO + WHI)/2;
01100		IF ¬(WLO<W ∧ W<WHI) THEN DONE;
01200		G	←	GAP(W);
01300		IF G⊗GLO ≥ 0 THEN
01400	BEGIN
01500		GLO	←	G;
01600		WLO	←	W;
01700	END	ELSE
01800	BEGIN
01900		GHI	←	G;
02000		WHI	←	W;
02100	END;
02200	END	"HALVE INTERVAL" UNTIL G=0 ∨ (WHI-WLO)<1/2↑25;
02400		RETURN(W);
02500	END	"ROOT";
     

00100	α SETUP FOR ROOT FINDING ITERATION;
00200		REAL DELTA,GOLD,G;
00300		ALPHA	←	ACOS(COSα);
00400		WLO 	←	ALPHA - π;
00500		WHI 	←	π - ALPHA;
00600		DELTA	←	(WHI-WLO)/200;
00700		PHIPSI(COSα,COSg,COSβ);
00800		G	←	GAP(WLO);
00900		NROOTS	←	0;
01000		FOR W←WLO STEP DELTA UNTIL WHI DO
01100	BEGIN	"INTERVAL"
01200		GOLD	←	G;
01300		G	←	GAP(W);
01400		IF G*GOLD < 0  ∧ ABS(G-GOLD)<1 THEN
01500	BEGIN	"ZERO CROSSING"
01600		WWW	←	ROOT(W-DELTA,W);
01700		NROOTS←NROOTS+1;
01800		OFRAME;
01900		V[NROOTS,1]←	X;
02000		V[NROOTS,2]←	Y;
02100		V[NROOTS,3]←	Z;
02200	END	"ZERO CROSSING";
02300	END	"INTERVAL";
02400		RETURN(IF NROOTS≠0 THEN NROOTS ELSE IF GOLD<0 THEN -1 ELSE 0);
02500	END	"3RD BLK";
02600	END	"2ND BLK";
02700	END	"LS1V3P";
02800	
02900	END	"LS1V3P";